// Quaternion
// (c) jimon game studio

#ifndef JEH_JEQUATERNION
#define JEH_JEQUATERNION

#include "jeTypes.h"
#include "jeVector3d.h"
#include "jeMatrix4.h"
#include "jeMath.h"

namespace je
{
	//! Quaternion
	template<typename T>
	class jeQuaternion
	{
	public:
		//! Components
		T X,Y,Z,W;

		//! Constructor
		jeQuaternion()
			:X(0),Y(0),Z(0),W(1)
		{
		}

		//! Constructor
		jeQuaternion(T x,T y,T z,T w)
			:X(x),Y(y),Z(z),W(w)
		{
		}

		//! Constructor
		jeQuaternion(T x)
			:X(x),Y(x),Z(x),W(x)
		{
		}

		//! Constructor
		template <class B>
		jeQuaternion(const jeQuaternion<B> & Other)
			:X((T)Other.X),Y((T)Other.Y),Z((T)Other.Z),W((T)Other.W)
		{
		}

		//! Constructor
		template <class B>
		jeQuaternion(const jeVector3d<B> & Other)
			:X((T)Other.X),Y((T)Other.Y),Z((T)Other.Z),W(0)
		{
		}

		//! Constructor
		template <class B>
		jeQuaternion(B Angle,const jeVector3d<B> & Axis)
		{
			T Sin,Cos;
			je::jeSinCos(Angle*0.5f,Sin,Cos);

			X = Axis.X * Sin;
			Y = Axis.Y * Sin;
			Z = Axis.Z * Sin;
			W = Cos;
		}

		//! Destructor
		~jeQuaternion()
		{
		}

		//! Operator +
		jeQuaternion<T> operator + () const
		{
			return *this;
		}

		//! Operator +
		jeQuaternion<T> operator + (const jeQuaternion<T> & Other) const
		{
			return jeQuaternion(X+Other.X,Y+Other.Y,Z+Other.Z,W+Other.W);
		}

		//! Operator -
		jeQuaternion<T> operator - () const
		{
			return jeQuaternion(-X,-Y,-Z,-W);
		}

		//! Operator -
		jeQuaternion<T> operator - (const jeQuaternion<T> & Other) const
		{
			return jeQuaternion(X-Other.X,Y-Other.Y,Z-Other.Z,W-Other.W);
		}

		//! Operator *
		jeQuaternion<T> operator * (const jeQuaternion<T> & Other) const
		{
			jeQuaternion Temp;
			Temp.W = (Other.W * W) - (Other.X * X) - (Other.Y * Y) - (Other.Z * Z);
			Temp.X = (Other.W * X) + (Other.X * W) + (Other.Y * Z) - (Other.Z * Y);
			Temp.Y = (Other.W * Y) + (Other.Y * W) + (Other.Z * X) - (Other.X * Z);
			Temp.Z = (Other.W * Z) + (Other.Z * W) + (Other.X * Y) - (Other.Y * X);
			return Temp;
		}

		//! Operator *
		template <class B>
		jeQuaternion<T> operator * (B Scalar) const
		{
			jeQuaternion Temp;
			Temp.X = X * Scalar;
			Temp.Y = Y * Scalar;
			Temp.Z = Z * Scalar;
			Temp.W = W * Scalar;
			return Temp;
		}

		//! Get Inverse
		inline jeQuaternion<T> GetInverse() const
		{
			return jeQuaternion(-X,-Y,-Z,W);
		}

		//! Rotate
		template <class B>
		void Rotate(jeVector3d<B> & Vector)
		{
			jeQuaternion Pos(Vector);
			jeQuaternion QuatInverted(-X,-Y,-Z,W);
			Pos  = (*this) * Pos * QuatInverted;
			Vector.SetXYZ(Pos.X,Pos.Y,Pos.Z);
		}

		//! Get Matrix
		inline jeMatrix4<T> GetMatrix() const
		{
			jeMatrix4<T> Out;
			Out.Identity();

			Out.Set(0,0,1.0f - 2.0f*Y*Y - 2.0f*Z*Z);
			Out.Set(1,0,2.0f*X*Y + 2.0f*Z*W);
			Out.Set(2,0,2.0f*X*Z - 2.0f*Y*W);
			Out.Set(0,1,2.0f*X*Y - 2.0f*Z*W);
			Out.Set(1,1,1.0f - 2.0f*X*X - 2.0f*Z*Z);
			Out.Set(2,1,2.0f*Z*Y + 2.0f*X*W);
			Out.Set(0,2,2.0f*X*Z + 2.0f*Y*W);
			Out.Set(1,2,2.0f*Z*Y - 2.0f*X*W);
			Out.Set(2,2,1.0f - 2.0f*X*X - 2.0f*Y*Y);

			return Out;
		}

		//! Dot Product
		inline f32 DotProduct(const jeQuaternion<T> & Other) const
		{
			return X*Other.X+Y*Other.Y+Z*Other.Z+W*Other.W;
		}

		//! Slerp
		//! interpolate this quaternion with Q
		jeQuaternion<T> Slerp(const jeQuaternion<T> & Q,T Time) const
		{
			jeQuaternion<T> Q1 = *this;
			jeQuaternion<T> Q2 = Q;
			jeQuaternion<T> Out;

			f32 Angle = Q1.DotProduct(Q2);

			if(Angle < 0.0f)
			{
				Q1 = Q1 * (-1.0f);
				Angle = -Angle;
			}

			f32 Scale,InvScale;

			const f32 EVP = 0.05f;

			if((Angle+1.0f) > EVP)
			{
				if((1.0f-Angle) >= EVP)
				{
					f32 Th = acosf(Angle);
					f32 InvSinTh = 1.0f / jeSin(Th);
					Scale = jeSin(Th * (1.0f-Time)) * InvSinTh;
					InvScale = jeSin(Th * Time) * InvSinTh;
				}
				else
				{
					Scale = 1.0f - Time;
					InvScale = Time;
				}
			}
			else
			{
				Q2.SetXYZW(-Q1.Y,Q1.X,-Q1.W,Q1.Z);
				Scale = jeSin(jePi * (0.5f - Time));
				InvScale = jeSin(jePi * Time);
			}

			Out = Q1 * Scale + Q2 * InvScale;
			return Out;
		}

		//! Set XYZW
		void SetXYZW(T x,T y,T z,T w)
		{
			X = x;
			Y = y;
			Z = z;
			W = w;
		}
	};

	//! f32 quaternion
	typedef jeQuaternion<f32> jeQuaternionf;
}

#endif